import pandas as pd
import numpy as np


################################################ 转换函数 #############################################
# 转换函数:经纬度转(x,y)（线性缩放至360x360）
def latlon_to_xy(lat, lon, lat_min, lat_max, lon_min, lon_max):
    # 经度方向（x轴）：从西到东（最小→最大）
    x = 260 * abs(lon - lon_min) / abs(lon_max - lon_min) + 45
    # 纬度方向（y轴）：从南到北（最小→最大）
    y = 260 * abs(lat - lat_min) / abs(lat_max - lat_min) + 40
    return x, y


# 逆转换函数:(x,y)转经纬度
def xy_to_latlon(x, y, lat_min, lat_max, lon_min, lon_max):
    # 逆向线性缩放公式
    # x = 260 * abs(lon - lon_min) / abs(lon_max - lon_min) + 45
    # -> (x - 45) * abs(lon_max - lon_min) / 260 = abs(lon - lon_min)
    # -> lon = lon_min + (x - 45) * (lon_max - lon_min) / 260
    lon = lon_min + (x - 45) * (lon_max - lon_min) / 260
    lat = lat_min + (y - 40) * (lat_max - lat_min) / 260
    return lat, lon


# 转换函数:(x,y)转(theta,phi)
def xy_to_polar(x, y):
    # 中心平移
    x_mid = x - 180
    y_mid = y - 180
    # 计算半径（防除零处理）
    r = np.sqrt(x_mid ** 2 + y_mid ** 2)
    r = np.where(r == 0, 1e-10, r)  # 当x=180且y=180时，设置极小值
    # 计算th（强制范围约束）
    th_ratio = np.clip(r / 180, -1.0, 1.0)  # 确保arcsin输入有效
    th_rad = np.arcsin(th_ratio)
    th_deg = np.degrees(th_rad)
    # 计算ph（带象限判断）
    ph_rad = np.arccos(x_mid / r)
    ph_deg = np.degrees(ph_rad)
    # 处理y_mid负向情况
    ph_deg = np.where(y_mid < 0, ph_deg + 180, ph_deg)
    # 角度范围修正（0-360度）
    th_deg = np.mod(th_deg, 360)
    ph_deg = np.mod(ph_deg, 360)
    return th_deg, ph_deg


# 逆转换函数:(theta,phi)转(x,y)
def polar_to_xy(th_deg, ph_deg):
    # 将角度转回弧度
    th_rad = np.radians(th_deg)
    ph_rad = np.radians(ph_deg)

    # 计算半径 r
    # th = arcsin(r/180) -> sin(th) = r/180 -> r = 180*sin(th)
    r = 180 * np.sin(th_rad)

    # 计算平移后的 x 和 y 坐标 (x_mid, y_mid)
    x_mid = r * np.cos(ph_rad)
    # 根据 ph 的象限恢复 y_mid 的符号
    # 当 ph 在 [0, 180) 时，y_mid >= 0; 当 ph 在 [180, 360) 时，y_mid < 0
    y_mid_sign = np.where((ph_deg >= 0) & (ph_deg < 180), 1, -1)
    y_mid = y_mid_sign * np.sqrt(np.maximum(r ** 2 - x_mid ** 2, 0))  # 使用 maximum 避免负数开方

    original_arccos_value = np.where(ph_deg > 180, ph_deg - 180, ph_deg)
    original_arccos_rad = np.radians(original_arccos_value)
    x_mid_corrected = r * np.cos(original_arccos_rad)

    # y_mid 的符号由 ph_deg > 180 判断
    y_mid_sign = np.where(ph_deg > 180, -1, 1)  # 如果 ph_deg > 180，则原始 y_mid < 0
    y_mid_corrected = y_mid_sign * np.sqrt(np.maximum(r ** 2 - x_mid_corrected ** 2, 0))

    # 恢复原始的 x 和 y 坐标
    x_corrected = x_mid_corrected + 180
    y_corrected = y_mid_corrected + 180

    return x_corrected, y_corrected


################################################ 对齐 #############################################
def lonlat_adjust(df_dataset, lon_smooth, lat_smooth):
    # 对齐 lon_smooth 和 lat_smooth 的第一个值
    # 获取原始真实数据的第一个值
    first_lon_real = df_dataset['lon_smooth'].iloc[0]
    first_lat_real = df_dataset['lat_smooth'].iloc[0]
    # 计算当前恢复序列第一个值与真实值之间的偏差
    delta_lon = first_lon_real - lon_smooth[0]
    delta_lat = first_lat_real - lat_smooth[0]
    # 对整个序列加上偏差（整体平移），实现对齐
    lon_smooth = lon_smooth + delta_lon
    lat_smooth = lat_smooth + delta_lat
    return lon_smooth, lat_smooth


################################################ 主函数 #############################################
def format_matlab_caekf(path_csv_dataset, path_csv_before, path_csv_after):
    # 1.读取CSV文件
    df = pd.read_csv(path_csv_before)

    # 2.格式化
    th_smooth = df['trace_th_smooth']
    ph_smooth = df['trace_ph_smooth']
    th_smooth = np.array(th_smooth)
    ph_smooth = np.array(ph_smooth)

    # 3. 从(theta,phi)转换回(x,y)和经纬度
    df = pd.read_csv(path_csv_dataset)
    lon_min, lon_max = df['lon_smooth'].min(), df['lon_smooth'].max()
    lat_min, lat_max = df['lat_smooth'].min(), df['lat_smooth'].max()
    # 俯仰角和方位角转换为直角坐标系(x,y)
    x_smooth, y_smooth = polar_to_xy(th_smooth, ph_smooth)
    # 直角坐标系(x,y)转换回经纬度
    lat_smooth_ori, lon_smooth_ori = xy_to_latlon(x_smooth, y_smooth, lat_min, lat_max, lon_min, lon_max)
    # 对齐 lon_smooth 和 lat_smooth 的第一个值
    lon_smooth, lat_smooth = lonlat_adjust(df, lon_smooth_ori, lat_smooth_ori)

    # 4. 创建含恢复结果的 DataFrame 并保存
    recovery_df = pd.DataFrame({
        'lon_smooth_ori': lon_smooth_ori,
        'lat_smooth_ori': lat_smooth_ori,
        'lon_smooth': lon_smooth,
        'lat_smooth': lat_smooth,
        'x_smooth': x_smooth,
        'y_smooth': y_smooth,
        'th_smooth': th_smooth,
        'ph_smooth': ph_smooth
    })

    recovery_df.to_csv(path_csv_after, index=False)
    print(f"恢复结果已保存到 {path_csv_after}")




if __name__=="__main__":
    format_matlab_caekf('uav_dataset_thph_xy_lonlat_dataset_test.csv', 'y_out_psll_point.csv',
                        "aeconvlstm_out_test_2_coord_2.csv")